

#include <ros/ros.h>
#include <sensor_msgs/Range.h>
#include <geometry_msgs/Twist.h>

#define _NODE_NAME "ultrasonic_obstacle_avoidance_node"
#define _SONA0_TOPIC_   "/robot0/sonar_0"
#define _SONA1_TOPIC_   "/robot0/sonar_1"
#define _SONA2_TOPIC_   "/robot0/sonar_2"

#define _STATE_A_        0x04
#define _STATE_B_        0x02
#define _STATE_C_        0x01
#define _STATE_D_        0x07
#define _STATE_E_        0x06
#define _STATE_F_        0x03
#define _STATE_G_        0x05


#define setbit(x, y)    x|=(1<<y)
#define clrbit(x, y)    x&=~(1<<y)

const double warn_range = 0.3;

double default_periad_hz = 10;
double default_linear_x = 0.5;
double default_yaw_rate = 0.5;

double range_array[3];

void sonar0_callback(const sensor_msgs::Range::ConstPtr &msg)
{
    ROS_INFO("sonar0 range:[%f]", msg->range);
    range_array[0] = msg->range;
}
void sonar1_callback(const sensor_msgs::Range::ConstPtr &msg)
{
    ROS_INFO("sonar1 range:[%f]", msg->range);
    range_array[1] = msg->range;
}
void sonar2_callback(const sensor_msgs::Range::ConstPtr &msg)
{
    ROS_INFO("sonar2 range:[%f]", msg->range);
    range_array[2] = msg->range;
}

ros::Publisher pub;
geometry_msgs::Twist twist_cmd;
void publishTwistCmd(double linear_x, double angular_z)
{
    twist_cmd.linear.x = linear_x;
    twist_cmd.linear.y = 0;
    twist_cmd.linear.z = 0;

    twist_cmd.angular.x = 0;
    twist_cmd.angular.y = 0;
    twist_cmd.angular.z = angular_z;

    pub.publish(twist_cmd);
}

void checkSonarRange(double sonar_l, double sonar_f, double sonar_r)
{
    unsigned char flag = 0;
    if(sonar_l < warn_range) setbit(flag, 2);
    else    clrbit(flag, 2);
    if(sonar_f < warn_range) setbit(flag, 1);
    else    clrbit(flag, 1);
    if(sonar_r < warn_range) setbit(flag, 0);
    else    clrbit(flag, 0);

    switch(flag)
    {
        case _STATE_A_:
            publishTwistCmd(0, -default_yaw_rate);
        break;
        case _STATE_B_:
            if(sonar_l > sonar_r) publishTwistCmd(0, default_yaw_rate);
            else publishTwistCmd(0, -default_yaw_rate);
        break;
        case _STATE_C_:
            publishTwistCmd(0, default_yaw_rate);
        break;
        case _STATE_D_:
            publishTwistCmd(0, 10*default_yaw_rate);
        break;
        case _STATE_E_:
            publishTwistCmd(0, -2*default_yaw_rate);
        break;
        case _STATE_F_:
            publishTwistCmd(0, 2*default_yaw_rate);
        break;
        case _STATE_G_:
            publishTwistCmd(2*default_linear_x, 0);
        break;
        default:
            publishTwistCmd(2*default_linear_x, 0);
        break;
    }
}

int main(int argc, char ** argv)
{
    ros::init(argc, argv, _NODE_NAME);
    ros::NodeHandle nh;

    ros::Subscriber sub0 = nh.subscribe(_SONA0_TOPIC_, 100, sonar0_callback);
    ros::Subscriber sub1 = nh.subscribe(_SONA1_TOPIC_, 100, sonar1_callback);
    ros::Subscriber sub2 = nh.subscribe(_SONA2_TOPIC_, 100, sonar2_callback);

    pub = nh.advertise<geometry_msgs::Twist>("/robot0/cmd_vel", 10);

    ros::Rate loop_rate(10);
    while(ros::ok())
    {
        checkSonarRange(range_array[0], range_array[1], range_array[2]);
        ros::spinOnce();
        loop_rate.sleep();
    }
    
    return 0;
}